/*
  IPG-50W Laser
*/


#ifndef ULAPI
#error This is intended as a userspace component only.
#endif

#ifdef DEBUG
#define DBG(fmt, ...)                   \
    do {                        \
    if (param.debug) printf(fmt,  ## __VA_ARGS__);  \
    } while(0)
#else
#define DBG(fmt, ...)
#endif

#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <string.h>
#include <ctype.h>
#include <stdlib.h>
#include <signal.h>
#include <errno.h>
#include <getopt.h>
#include <math.h>
#include <signal.h>
#include <stdarg.h>
#include <stdbool.h>

#include "rtapi.h"
#include "hal.h"
#include <modbus.h>
#include <modbus-tcp.h>
#include "inifile.h"

#include <termios.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <sys/select.h>



// command code
//read code
#define DEV_ID                  1   // 
#define DEV_SN                  2   // 
#define FW_REV                  3   //Read device firmware revision  
#define VENDOR                  99  // 
#define DEV_STATUS              4   // device state 
#define DEV_TEMP                5   // Read module temperature in degree Celsius 
#define DIG_INTERFACE_STATUS    10  // Reads digital interface status, decimal to binary decoding is required  
#define EXTEND_STATUS           11  // 
#define BR_COUNTER              12  // 
#define SESSION_BR_COUNTER      13  //
#define NOM_AVERAGE_POWER       14  //  
#define NOM_PULSE_POWER         15  //
#define NOM_PULSE_ENERGY        16  //
#define NOM_PEAK_POWER          17  //
#define PRR_RANGE               18  //
#define HEAD_TEMP               19  //
#define MAIN_SUPPLY_VOLTAGE     21  //
#define VOLTAGE_24V             22  //
#define OPERATE_MODE            23  //
#define INSTALL_OPTIONS         25  //
#define START_OPERATE_MODE      27  //
#define OPERATING_POWER_W         33    //
#define OPERATING_POWER_PERCENTAGE   34 // %
#define OPERATING_PULSE_ENERGY       36 //
#define PRR_MONITOR                38   //
#define ALARM_COUNTERS             70   //
#define MODULE_TEMP_RANG           58   //
#define NOM_FREQUENCY              59   //
#define CRITICAL_ERROR_COUNTER     95   //
#define CRITICAL_ERROR_CODE        96   //
#define READ_PRR                   29   //
#define APD_MODES_NUMBER           55   //
#define APD_MODE_DESCRIPTION       56   //
#define APD_MODE_INDEX             68   //
#define READ_TEST                  51   //
#define PULSE_DURATION             48   //



//Write code
#define SET_OPERATING_MODE          24  //
#define SET_START_OPERATING_MODE      26    //
#define RESET_CRITICAL_ERROR_ALARM  97  //
#define SET_PRR                     28  //
#define SET_EMISSION_ON             30  //
#define SET_EMISSION_OFF            31  //
#define SET_OPERATING_POWER         32  //
#define SET_GUIDE_ON                40  //GUIDE LASER ON
#define SET_GUIDE_OFF               41  //
#define SET_EE_ON                   42  //SWITCH ON EMISSION ENABLE
#define SET_EE_OFF                  43  //SWITCH OFF EMISSION ENABLE
#define RESET_ALARMS                50  //EMISSION ENABLE
#define SET_APD_MODE_INDEX          69  //
#define SAVE_APD_MODE_INDEX         54  //
#define SET_PULSE_DURATION          49   //

#define BUFFER_LENGTH               512   //


//typedef enum
//{
//    PULSE  = 7,
//    CONTINUOUS = 8,
//} LASER_MODEL;


/* HAL data struct */
typedef struct
{
    //Laser output
    hal_u32_t   *status;
    hal_float_t *device_temperature;
//    hal_u32_t   *digital_interface_status;
    hal_u32_t   *extend_status;
//    hal_u32_t   *br_counter;
//    hal_u32_t   *session_br_counter;
//    hal_float_t *nom_average_power;
//    hal_float_t *nom_pulse_duration;
//    hal_float_t *nom_pulse_energy;
//    hal_float_t *nom_peak_power;
//    hal_float_t *min_prr; //prr range
//    hal_float_t *max_prr; //prr range
//    hal_float_t *head_temperature;
    hal_float_t *main_supply_voltage;
    hal_float_t *voltage_24v;
    hal_u32_t   *operator_mode;
//    hal_u32_t   *installed_options;
//    hal_u32_t   *start_operator_mode;
    hal_float_t *operate_power_w;
    hal_float_t *operate_power_percentage;
    hal_float_t *operate_pulse_energy;
//    hal_float_t *prr_monitor;
    hal_u32_t   *alarm_counters;//16bit integer
    hal_float_t *min_module_tempertaure;
    hal_float_t *max_module_tempertaure;
    hal_float_t *nom_frequency;
    hal_u32_t   *critical_error_counter;//16bit integer
    hal_u32_t   *critical_error_code;
    hal_float_t *prr;
//    hal_u32_t   *apd_modes_number;//16bit integer
    hal_u32_t   *apd_mode_index; //16bit integer
    hal_s32_t   *pulse_duration;  //16bit integer
    hal_bit_t   *modbus_ok; // the last MODBUS_OK transactions returned successfully

    //Laser input
//    hal_u32_t   *operator_mode_cmd;
//    hal_u32_t   *start_operator_mode_cmd;
//    hal_u32_t   *reset_critical_error_alarm_cmd;
    hal_float_t *prr_cmd;
    hal_bit_t   *emission_cmd;  // 0 off   1 on
    hal_bit_t   *guide_cmd; // 0 off   1 on
    hal_bit_t   *ee_cmd;    // 0 off   1 on  emission enable
    hal_float_t *operate_power_percentage_cmd; // %  0-100
    hal_bit_t   *reset_alarms_cmd;  // 0 do nothing   1 reset alarms
//    hal_u32_t   *apd_modes_index_cmd;//16bit integer
//    hal_bit_t   *save_apd_modes_index_cmd;//
    hal_float_t   *apd_mode_index_cmd; //16bit integer
    hal_bit_t   *enabled;

    //
    hal_float_t looptime;
    hal_float_t speed_tolerance;

} haldata_t;

// configuration and execution state
typedef struct params
{
    int type;
    char *modname;
    int pollcycles;
    char *device;
    int baud;
    int bits;
    char parity;
    char *stopbits;
    int rts_mode;
    int serial_mode;
    struct timeval response_timeout;
    struct timeval byte_timeout;
    char *progname;
    char *section;
    FILE *fp;
    char *inifile;
    int reconnect_delay;
    int fd;
    haldata_t *haldata;
    int hal_comp_id;
    int read_initial_done;
    int old_err_reset;
    uint16_t old_cmd1_reg;      // copy of last write to FA00 */
    int modbus_ok;
    int last_errno;
    int report_device;

    float  old_apd_mode_index;//16bit integer
    float     old_prr;
    float     old_operate_power_percentage; // %  0-100
    volatile bool      old_guide;
    volatile bool      old_ee;
    volatile bool      old_emission;
} params_type, *param_pointer;

#define TYPE_RTU 0
#define TYPE_TCP_SERVER 1
#define TYPE_TCP_CLIENT 2
#define TYPE_CUSTOM     3

#define POLLCYCLES  10
#define MODBUS_MIN_OK   10


char stop_bits = '1';

struct termios old_tios;


// default options; read from inifile or command line
static params_type param =
{
    .type = -1,
    .modname = NULL,
    .pollcycles = POLLCYCLES,
    .device = "/dev/ttyS0",
    .baud = 57600,
    .bits = 8,
    .parity = 'N',
    .stopbits = &stop_bits,
    .serial_mode = -1,
    .rts_mode = -1,
    .response_timeout = { .tv_sec = 0, .tv_usec = 500000 },
    .byte_timeout = {.tv_sec = 0, .tv_usec = 500000},
    .progname = "ipg-laser",
    .section = "IPG",
    .fp = NULL,
    .inifile = NULL,
    .reconnect_delay = 1,
    .fd = -1,
    .haldata = NULL,
    .hal_comp_id = -1,
    .read_initial_done = 0,
    .old_err_reset = 0,
    .old_cmd1_reg = 0,
    .modbus_ok = 0,    // set modbus-ok bit if last MODBUS_OK transactions went well
    .last_errno = 0,
    .report_device = 0,
    .old_apd_mode_index = 0,
    .old_prr = 0.0,
    .old_operate_power_percentage = 0.0,
    .old_guide = false,
    .old_ee    = false,
    .old_emission = false,
};


static int connection_state;
enum connstate {NOT_CONNECTED, OPENING, CONNECTING, CONNECTED, RECOVER, DONE};

static char *option_string = "dhrmn:S:I:";
static struct option long_options[] =
{
    {"debug", no_argument, 0, 'd'},
    {"help", no_argument, 0, 'h'},
    {"modbus-debug", no_argument, 0, 'm'},
    {"report-device", no_argument, 0, 'r'},
    {"ini", required_argument, 0, 'I'},     // default: getenv(INI_FILE_NAME)
    {"section", required_argument, 0, 'S'}, // default section = LIBMODBUS
    {"name", required_argument, 0, 'n'},    // vfs11_vfd
    {0,0,0,0}
};


void  windup(param_pointer p)
{
	if (p->fd)
			tcsetattr(p->fd, TCSANOW, &old_tios);
			close(p->fd);

    if (p->hal_comp_id >= 0)
        hal_exit(p->hal_comp_id);
    
}

static void toggle_modbus_debug(int sig)
{
//    param.modbus_debug = !param.modbus_debug;
//    modbus_set_debug(param.ctx, param.modbus_debug);
}

static void toggle_debug(int sig)
{
//    param.debug = !param.debug;
}

static void quit(int sig)
{

    switch (connection_state)
    {

        case CONNECTING:
            // modbus_tcp_accept() or TCP modbus_connect()  were interrupted
            // these wont return to the main loop, so exit here
            windup(&param);
            exit(0);
            break;

        default:
            connection_state = DONE;
            break;
    }
}

enum kwdresult {NAME_NOT_FOUND, KEYWORD_INVALID, KEYWORD_FOUND};
#define MAX_KWD 10

int findkwd(param_pointer p, const char *name, int *result, const char *keyword, int value, ...)
{
    const char *word;
    va_list ap;
    const char *kwds[MAX_KWD], **s;
    int nargs = 0;

    if ((word = iniFind(p->fp, name, p->section)) == NULL)
        return NAME_NOT_FOUND;

    kwds[nargs++] = keyword;
    va_start(ap, value);

    while (keyword != NULL)
    {
        if (!strcasecmp(word, keyword))
        {
            *result = value;
            va_end(ap);
            return KEYWORD_FOUND;
        }
        keyword = va_arg(ap, const char *);
        kwds[nargs++] = keyword;
        if (keyword)
            value = va_arg(ap, int);
    }
    fprintf(stderr, "%s: %s:[%s]%s: found '%s' - not one of: ",
            p->progname, p->inifile, p->section, name, word);
    for (s = kwds; *s; s++)
        fprintf(stderr, "%s ", *s);
    fprintf(stderr, "\n");
    va_end(ap);
    return KEYWORD_INVALID;
}

int read_ini(param_pointer p)
{
    const char *s;
//    const char *word;
    double f;
    int value;

    if ((p->fp = fopen(p->inifile,"r")) != NULL)
    {

        iniFindInt(p->fp, "BITS", p->section, &p->bits);
        iniFindInt(p->fp, "BAUD", p->section, &p->baud);
//        iniFindInt(p->fp, "TARGET", p->section, &p->slave);
        iniFindInt(p->fp, "POLLCYCLES", p->section, &p->pollcycles);
//        iniFindInt(p->fp, "PORT", p->section, &p->tcp_portno);
        iniFindInt(p->fp, "RECONNECT_DELAY", p->section, &p->reconnect_delay);
        p->stopbits = iniFind(p->fp, "STOPBITS", p->section);

        if ((s = iniFind(p->fp, "DEVICE", p->section)))
        {
            p->device = strdup(s);
        }
        if (iniFindDouble(p->fp, "RESPONSE_TIMEOUT", p->section, &f))
        {
            p->response_timeout.tv_sec = (int) f;
            p->response_timeout.tv_usec = (f-p->response_timeout.tv_sec) * 1000000;
        }
        if (iniFindDouble(p->fp, "BYTE_TIMEOUT", p->section, &f))
        {
            p->byte_timeout.tv_sec = (int) f;
            p->byte_timeout.tv_usec = (f-p->byte_timeout.tv_sec) * 1000000;
        }
        value = p->parity;
        if (findkwd(p, "PARITY", &value,
                    "even",'E',
                    "odd", 'O',
                    "none", 'N',
                    NULL) == KEYWORD_INVALID)
            return -1;
        p->parity = value;

        if (findkwd(p,"SERIAL_MODE", &p->serial_mode,
                    "rs232", MODBUS_RTU_RS232,
                    "rs485", MODBUS_RTU_RS485,
                    NULL) == KEYWORD_INVALID)
            return -1;

        if (findkwd(p, "TYPE", &p->type,
                    "rtu", TYPE_RTU,
                    "tcpserver", TYPE_TCP_SERVER,
                    "tcpclient", TYPE_TCP_CLIENT,
                    "custom",TYPE_CUSTOM,
                    NULL) == NAME_NOT_FOUND)
        {
            fprintf(stderr, "%s: missing required TYPE in section %s\n",
                    p->progname, p->section);
            return -1;
        }
    }
    else
    {
        fprintf(stderr, "%s:cant open inifile '%s'\n",
                p->progname, p->inifile);
        return -1;
    }
    return 0;
}

void usage(int argc, char **argv)
{
    printf("Usage:  %s [options]\n", argv[0]);
    printf("This is a userspace HAL program, typically loaded using the halcmd \"loadusr\" command:\n"
           "    loadusr vfs11_vfd [options]\n"
           "Options are:\n"
           "-I or --ini <inifile>\n"
           "    Use <inifile> (default: take ini filename from environment variable INI_FILE_NAME)\n"
           "-S or --section <section-name> (default 8)\n"
           "    Read parameters from <section_name> (default 'VFS11')\n"
           "-d or --debug\n"
           "    Turn on debugging messages. Toggled by USR1 signal.\n"
           "-m or --modbus-debug\n"
           "    Turn on modbus debugging.  This will cause all modbus messages\n"
           "    to be printed in hex on the terminal. Toggled by USR2 signal.\n"
           "-r or --report-device\n"
           "    Report device properties on console at startup\n");
}

static int set_baudrate(int fd,struct termios *opt, unsigned int baudrate)
{
    speed_t speed;

    switch (baudrate)
    {
        case 110:
            speed = B110;
            break;
        case 300:
            speed = B300;
            break;
        case 600:
            speed = B600;
            break;
        case 1200:
            speed = B1200;
            break;
        case 2400:
            speed = B2400;
            break;
        case 4800:
            speed = B4800;
            break;
        case 9600:
            speed = B9600;
            break;
        case 19200:
            speed = B19200;
            break;
        case 38400:
            speed = B38400;
            break;
#ifdef B57600
        case 57600:
            speed = B57600;
            break;
#endif
#ifdef B115200
        case 115200:
            speed = B115200;
            break;
#endif
#ifdef B230400
        case 230400:
            speed = B230400;
            break;
#endif
#ifdef B460800
        case 460800:
            speed = B460800;
            break;
#endif
#ifdef B500000
        case 500000:
            speed = B500000;
            break;
#endif
#ifdef B576000
        case 576000:
            speed = B576000;
            break;
#endif
#ifdef B921600
        case 921600:
            speed = B921600;
            break;
#endif
#ifdef B1000000
        case 1000000:
            speed = B1000000;
            break;
#endif
#ifdef B1152000
        case 1152000:
            speed = B1152000;
            break;
#endif
#ifdef B1500000
        case 1500000:
            speed = B1500000;
            break;
#endif
#ifdef B2500000
        case 2500000:
            speed = B2500000;
            break;
#endif
#ifdef B3000000
        case 3000000:
            speed = B3000000;
            break;
#endif
#ifdef B3500000
        case 3500000:
            speed = B3500000;
            break;
#endif
#ifdef B4000000
        case 4000000:
            speed = B4000000;
            break;
#endif
        default:
            speed = B9600;
            break;
    }

    if ((cfsetispeed(opt, speed) < 0) ||
        (cfsetospeed(opt, speed) < 0))
    {
        close(fd);
        fd = -1;
        return -1;
    }
    return  0;
}


static void set_stopbit(struct termios *opt, const char *stopbit)
{
    if (0 == strcmp (stopbit, "1"))
    {
        opt->c_cflag &= ~CSTOPB;
    }
    else if (0 == strcmp (stopbit, "1.5"))
    {
        opt->c_cflag &= ~CSTOPB;
    }
    else if (0 == strcmp (stopbit, "2"))
    {
        opt->c_cflag |= CSTOPB;
    }
    else
    {
        opt->c_cflag &= ~CSTOPB;
    }
}


static void set_data_bit(struct termios *opt, unsigned int databit)
{
    opt->c_cflag |= (CREAD | CLOCAL);
    opt->c_cflag &= ~CSIZE;
    switch (databit)
    {
        case 8:
            opt->c_cflag |= CS8;
            break;
        case 7:
            opt->c_cflag |= CS7;
            break;
        case 6:
            opt->c_cflag |= CS6;
            break;
        case 5:
            opt->c_cflag |= CS5;
            break;
        default:
            opt->c_cflag |= CS8;
            break;
    }
}


static void set_parity(struct termios *opt, char parity)
{
    switch (parity)
    {
        case 'N':
        case 'n':
            opt->c_cflag &= ~PARENB;
            break;
        case 'E':
        case 'e':
            opt->c_cflag |= PARENB;
            opt->c_cflag &= ~PARODD;
            break;
        case 'O':
        case 'o':
            opt->c_cflag |= PARENB;
            opt->c_cflag |= ~PARODD;
            break;
        default:
            opt->c_cflag &= ~PARENB;
            break;
    }
}


int  set_port_attr(int fd,int baudrate, int databit, const char *stopbit, char parity, int vtime,int vmin )
{
    struct termios opt;
    tcgetattr(fd, &opt);

    if(set_baudrate(fd,&opt, baudrate) < 0)
    {
        return -1;

    }
    set_data_bit(&opt, databit);
    set_parity(&opt, parity);
    set_stopbit(&opt, stopbit);

    opt.c_cflag &= ~CRTSCTS;
    opt.c_cflag |= CLOCAL | CREAD;

    if (parity == 'N')
    {
        /* None */
        opt.c_iflag &= ~INPCK;
    }
    else
    {
        opt.c_iflag |= INPCK;
    }

    //opt.c_iflag &= ~(IXON | IXOFF | IXANY| ICRNL | IGNCR);
	opt.c_iflag &=  ~(IXON | IXOFF | IXANY);
    opt.c_oflag &= ~OPOST;

    opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); //
    opt.c_cc[VMIN] = vmin;
    opt.c_cc[VTIME] = vtime;
//    tcflush (fd, TCIFLUSH);
    if (tcsetattr(fd, TCSANOW, &opt) < 0)
    {
    	tcsetattr(fd, TCSANOW, &old_tios);
        close(fd);
        fd = -1;
        return -1;
    }
    return 0;
}

static int recv_data(int fd, void *buf, int length_to_read )
{
    int rc;
    int s_rc;
    fd_set rset;
    struct timeval tv;
    struct timeval *p_tv;
    unsigned char frame_end = 0;
    int msg_length = 0;
    unsigned char *recv_data = (unsigned char *)buf;

    while (frame_end != '\r')
    {
        FD_ZERO(&rset);  //need to clear everytime
        FD_SET(fd, &rset);

        tv.tv_sec = 3;
        tv.tv_usec = 0;
        p_tv = &tv;
        while ((s_rc = select(fd+1,&rset, NULL, NULL, p_tv)) == -1)
        {
            if (errno == EINTR)
            {

                FD_ZERO(&rset);
                FD_SET(fd, &rset);
            }
            else
            {
                return -1;
            }
        }

        if (s_rc == 0)
        {
            return -1;
        }
        if(FD_ISSET(fd,&rset))
        {
            rc = read(fd, recv_data + msg_length, length_to_read);
            if (rc == 0)
            {
                rc = -1;
            }

            if (rc == -1)
            {
                return -1;
            }

            /* Sums bytes received */
            msg_length += rc;
            length_to_read -= rc;
            if(msg_length > 1)
            {
                frame_end = recv_data[msg_length-1];
            }
        }

    }

    return msg_length;

}

static int recv_data_bak(int fd, void *buf, int length_to_read )
{
    
    int msg_length = 0;   
    msg_length  = read(fd, (unsigned char *)buf , length_to_read);

    return msg_length;

}


//init 1
static int get_device_temperatur(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','5',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,4);
    //usleep(175 * 4 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_device_temperatur send error");
        return -1;
    }
//    int size = read(fd, Recv_buffer, 20);
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    char *strchr_pointer = strchr(Recv_buffer, ';' );
    Recv_buffer[size-1] = 0;
    *haldata->device_temperature = (float)atof((const char *)++strchr_pointer);
    return 0;

}

//init 2
static int set_installed_option(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','2','5',';','7','7','\r',0};
    char send_buffer_exten[20] = {'$','2','5',';','1','\r',0};

    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,7);
    //usleep(175 * 7 + 700);
    if(len < 0)
    {
        fprintf(stderr,"set_installed_option 77 send error");
        return -1;
    }
//    int size = read(fd, Recv_buffer, 20);
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_installed_option 77 receive error");
        return -1;
    }
    len = write(fd,send_buffer_exten,6);
    if(len < 0)
    {
        fprintf(stderr,"set_installed_option 1 send error");
        return -1;
    }
    size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_installed_option 1 receive error");
        return -1;
    }
    return 0;

}

//init 3
static int get_read_test(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','5','1',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
//    int size = read(fd, Recv_buffer, 20);
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }

    return 0;

}

//init 4    ns
static int get_pulse_duratin(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','4','8',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    char *strchr_pointer = strchr(Recv_buffer, ';' );
    Recv_buffer[size-1] = 0;
    *haldata->pulse_duration = (float)atof((const char *)++strchr_pointer);

    return 0;

}

//init 5  ns
static int set_pulse_duratin(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','4','9',';','4','0','0','0','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,9);
    //usleep(175 * 9 + 700);
    if(len < 0)
    {
        fprintf(stderr,"set_pulse_duratin send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }

    return 0;

}

//init 6    KHz
static int get_prr_range(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','1','8',';','\r',0};
    char Recv_buffer[40] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
//    int size = read(fd, Recv_buffer, 20);
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }

    char *strchr_pointer = strchr(Recv_buffer, ';' );
    strchr_pointer = strchr(++strchr_pointer, ';' );
    Recv_buffer[size-1] = 0;
    *strchr_pointer = 0;
//    *haldata->max_prr = (float)atof((const char *)++strchr_pointer);
    strchr_pointer = strchr(Recv_buffer, ';' );
//    *haldata->min_prr = (float)atof((const char *)++strchr_pointer);
    return 0;
}

//init 7  KHz
static int get_nom_frequency(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','5','9',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    char *strchr_pointer = strchr(Recv_buffer, ';' );
    Recv_buffer[size-1] = 0;
    *haldata->nom_frequency = (float)atof((const char *)++strchr_pointer);

    return 0;

}

//init 8  KHz
static int get_prr(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','2','9',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    char *strchr_pointer = strchr(Recv_buffer, ';' );
    Recv_buffer[size-1] = 0;
    *haldata->prr = (float)atof((const char *)++strchr_pointer);

    return 0;

}

//init 9
static int get_device_status(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','4',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,4);
    //usleep(175 * 4 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    char *strchr_pointer = strchr(Recv_buffer, ';' );
    Recv_buffer[size-1] = 0;
    *haldata->status = (uint32_t)atoi((const char *)++strchr_pointer);

    return 0;


}

//init 10
static int get_extend_status(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','1','1',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    char *strchr_pointer = strchr(Recv_buffer, ';' );
    Recv_buffer[size-1] = 0;
    *haldata->extend_status = (uint32_t)atoi((const char *)++strchr_pointer);

    return 0;
}

//init 11
static int get_seission_counter(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','1','3',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    char *strchr_pointer = strchr(Recv_buffer, ';' );
    Recv_buffer[size-1] = 0;
//    *haldata->session_br_counter = (uint32_t)atoi((const char *)++strchr_pointer);

    return 0;
}

static int set_apd_model(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','6','9',';','7','\r',0};
    char Recv_buffer[20] = {0};
    char text[6] = {0};

    int num = sprintf(text,"%1d",(uint32_t)(*haldata->apd_mode_index_cmd));
    if(-1 == num)
    {
        fprintf(stderr,"set_apd_model index error");
        return -1;
    }
    send_buffer[4] = text[0];
    int len = write(fd,send_buffer,6);
    //usleep(175 * 6 + 700);
    if(len < 0)
    {
        fprintf(stderr,"set_apd_model send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_apd_model receive error");
        return -1;
    }

    return 0;

}

//
static int set_emission_enable(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','4','3',';','\r',0};
    char Recv_buffer[20] = {0};


    if(*haldata->ee_cmd == true)
    {
        send_buffer[2] = '2';
    }

    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"set_emission_enable send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_emission_enable receive error");
        return -1;
    }

    return 0;

}

//set pulse frequency
//float, 1 digit after point
//Set operating pulse repetition rate in [kHz]
static int set_prr(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','2','8',';',0,0,0};
    char Recv_buffer[20] = {0};
    char text[20] = {0};
    int index = 0;

    int num = sprintf(text,"%.1f",*haldata->prr_cmd);
    if(-1 == num)
    {
        fprintf(stderr,"set_prr index error");
        return -1;
    }
    for(int i=4,j=0; j<num; i++,j++)
    {
        send_buffer[i] =  text[j];
        index = i;
    }
    send_buffer[index] = '\r';

    int len = write(fd,send_buffer,++index);
    //usleep(175 * index + 700);
    if(len < 0)
    {
        fprintf(stderr,"set_prr send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_prr receive error");
        return -1;
    }

    return 0;
}

//set operating power [%]
static int set_operating_power(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','3','2',';',0,0,0};
    char Recv_buffer[20] = {0};
    char text[10] = {0};
    int index = 0;

    int num = sprintf(text,"%.1f",*haldata->operate_power_percentage_cmd);
    if(-1 == num)
    {
        fprintf(stderr,"set_operating_power percent error");
        return -1;
    }
    for(int i=4,j=0; j<num; i++,j++)
    {
        send_buffer[i] =  text[j];
        index = i;
    }
    send_buffer[index] = '\r';

    int len = write(fd,send_buffer,++index);
    //usleep(175 * index + 700);
    if(len < 0)
    {
        fprintf(stderr,"set_operating_power send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_operating_power receive error");
        return -1;
    }

    return 0;
}

//get operating power
static int get_operating_power(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','3','4',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    char *strchr_pointer = strchr(Recv_buffer, ';' );
    Recv_buffer[size-1] = 0;
    *haldata->operate_power_percentage = (float)atof((const char *)++strchr_pointer);

    return 0;

}

static int get_apd_model(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','6','8',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    char *strchr_pointer = strchr(Recv_buffer, ';' );
    Recv_buffer[size-1] = 0;
    *haldata->apd_mode_index = (uint32_t)atoi((const char *)++strchr_pointer);
    return 0;

}

static int set_guiding_light(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','4','0',';','\r',0};
    char Recv_buffer[20] = {0};


    if(*haldata->guide_cmd == false)
    {
        send_buffer[2] = '1';
    }

    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"set_guiding_light send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_guiding_light receive error");
        return -1;
    }

    return 0;

}

static int set_laser_emission(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','3','1',';','\r',0};
    char Recv_buffer[20] = {0};


    if(*haldata->emission_cmd == true)
    {
        send_buffer[2] = '0';
    }

    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"set_laser_emission send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"set_laser_emission receive error");
        return -1;
    }

    return 0;

}

static int reset_alarm(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','5','0',';','\r',0};
//    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    return 0;
}

static int get_prr_monitor(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'$','3','8',';','\r',0};
    char Recv_buffer[20] = {0};
    int len = write(fd,send_buffer,5);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    char *strchr_pointer = strchr(Recv_buffer, ';' );
    Recv_buffer[size-1] = 0;
//    *haldata->prr_monitor = (float)atof((const char *)++strchr_pointer);
    return 0;

}

static int read_initial(int fd, haldata_t *haldata, param_pointer p)
{
    int retval = get_device_temperatur(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = set_installed_option(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = get_read_test(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = get_pulse_duratin(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = set_pulse_duratin(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = get_prr_range(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = get_nom_frequency(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = get_prr(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = get_device_status(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = get_extend_status(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = get_seission_counter(fd,haldata,p);
    if(retval < 0)
        return 1;

    return 0;

}

static int read_data(int fd, haldata_t *haldata, param_pointer p)
{
    int retval;
    static int Cnt = 0;

    if (!p->read_initial_done)
    {
        if ((retval = read_initial(fd, haldata, p)))
            return retval;
        else
            p->read_initial_done = 1;
    }

    // we always at least read the main status register SR_INV_OPSTATUS
    switch(Cnt)
    {
        case 0:
            retval = get_prr(fd,haldata,p);
            Cnt++;
            break;
        case 1:
            retval = get_operating_power(fd,haldata,p);
            Cnt++;
            break;
        case 2:
            retval = get_prr_monitor(fd,haldata,p);
            Cnt++;
            break;
        case 3:
            retval = get_device_status(fd,haldata,p);
            Cnt++;
            break;
        case 4:
            retval = get_extend_status(fd,haldata,p);
            Cnt++;
            break;
        case 5:
            retval = get_seission_counter(fd,haldata,p);
            Cnt++;
            break;
        case 6:
            retval = get_apd_model(fd,haldata,p);
            Cnt = 0;
            break;
    }
    if(retval  < 0)
        return 1;
    return 0;
}

static int write_data(int fd, haldata_t *haldata, param_pointer p)
{
    //waiting for Laser ready
    if (!*(haldata->enabled))
    {
        return 0;
    }

    if(*haldata->apd_mode_index_cmd != p->old_apd_mode_index)
    {
        if(set_apd_model(fd,haldata,p) < 0)
            return 1;
        p->old_apd_mode_index = *haldata->apd_mode_index_cmd;
        get_apd_model(fd,haldata,p);
    }
    if(*haldata->prr_cmd != p->old_prr)
    {
        if(set_prr(fd,haldata,p) < 0)
            return 1;
        p->old_prr = *haldata->prr_cmd;
        get_prr(fd,haldata,p);
    }
    if(*haldata->operate_power_percentage_cmd != p->old_operate_power_percentage)
    {
        if(set_operating_power(fd,haldata,p) < 0)
            return 1;
        p->old_operate_power_percentage = *haldata->operate_power_percentage_cmd;
        get_operating_power(fd,haldata,p);
    }

    if(*haldata->guide_cmd != p->old_guide)
    {
        if(set_guiding_light(fd,haldata,p) < 0)
            return 1;
        p->old_guide = *haldata->guide_cmd;
    }

    if(*haldata->ee_cmd != p->old_ee)
    {
        if(set_emission_enable(fd,haldata,p) < 0)
            return 1;
        p->old_ee = *haldata->ee_cmd;
    }

    if(*haldata->emission_cmd != p->old_emission)
    {
        if(set_laser_emission(fd,haldata,p) < 0)
            return 1;
        p->old_emission = *haldata->emission_cmd;
    }

    return 0;
}


#define PIN(x)                  \
    do {                        \
    status = (x);                   \
    if ((status) != 0)              \
        return status;              \
    } while (0)

static int hal_setup(int id, haldata_t *h, const char *name)
{
    int status;
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->status), id, "%s.device-status", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->device_temperature), id, "%s.device-temperature", name));
//    PIN(hal_pin_u32_newf(HAL_OUT, &(h->digital_interface_status), id, "%s.digital-interface-status", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->extend_status), id, "%s.extend-status", name));
//    PIN(hal_pin_u32_newf(HAL_OUT, &(h->br_counter), id, "%s.br-counter", name));
//    PIN(hal_pin_u32_newf(HAL_OUT, &(h->session_br_counter), id, "%s.session-br-counter", name));
//    PIN(hal_pin_float_newf(HAL_OUT, &(h->nom_average_power), id, "%s.nom-average-power", name));
//    PIN(hal_pin_float_newf(HAL_OUT, &(h->nom_pulse_duration), id, "%s.nom-pulse-duration", name));
//    PIN(hal_pin_float_newf(HAL_OUT, &(h->nom_pulse_energy), id, "%s.nom-pulse-energy", name));
//    PIN(hal_pin_float_newf(HAL_OUT, &(h->nom_peak_power), id, "%s.nom-peak-power", name));
//    PIN(hal_pin_float_newf(HAL_OUT, &(h->min_prr), id, "%s.min-prr", name));
//    PIN(hal_pin_float_newf(HAL_OUT, &(h->max_prr), id, "%s.max-prr", name));
//    PIN(hal_pin_float_newf(HAL_OUT, &(h->head_temperature), id, "%s.head-temperature", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->main_supply_voltage), id, "%s.main-supply-voltage", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->voltage_24v), id, "%s.voltage-24v", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->operator_mode), id, "%s.operator-mode", name));
//    PIN(hal_pin_u32_newf(HAL_OUT, &(h->installed_options), id, "%s.installed-options", name));
//    PIN(hal_pin_u32_newf(HAL_OUT, &(h->start_operator_mode), id, "%s.start-operator-mode", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->operate_power_w), id, "%s.operate-power-w", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->operate_power_percentage), id, "%s.operate-power-percentage", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->operate_pulse_energy), id, "%s.operate-pulse-energy", name));
//    PIN(hal_pin_float_newf(HAL_OUT, &(h->prr_monitor), id, "%s.prr-monitor", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->alarm_counters), id, "%s.alarm-counters", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->min_module_tempertaure), id, "%s.min-module-tempertaure", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->max_module_tempertaure), id, "%s.max-module-tempertaure", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->nom_frequency), id, "%s.nom-frequency", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->critical_error_counter), id, "%s.critical-error-counter", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->critical_error_code), id, "%s.critical-error-code", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->prr), id, "%s.prr", name));
//    PIN(hal_pin_u32_newf(HAL_OUT, &(h->apd_modes_number), id, "%s.apd-modes-number", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->apd_mode_index), id, "%s.apd-mode-index", name));
    PIN(hal_pin_s32_newf(HAL_OUT, &(h->pulse_duration), id, "%s.pulse-duration", name));
    PIN(hal_pin_bit_newf(HAL_OUT, &(h->modbus_ok), id, "%s.modbus-ok", name));

//    PIN(hal_pin_u32_newf(HAL_IN, &(h->operator_mode_cmd), id, "%s.operator-modecmd", name));
//    PIN(hal_pin_u32_newf(HAL_IN, &(h->start_operator_mode_cmd), id, "%s.start-operator-modecmd", name));
//    PIN(hal_pin_u32_newf(HAL_IN, &(h->reset_critical_error_alarm_cmd), id, "%s.reset-critical-error-alarmcmd", name));
    PIN(hal_pin_float_newf(HAL_IN, &(h->prr_cmd), id, "%s.prr-cmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->emission_cmd), id, "%s.emission-cmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->guide_cmd), id, "%s.guide-cmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->ee_cmd), id, "%s.ee-cmd", name));
    PIN(hal_pin_float_newf(HAL_IN, &(h->operate_power_percentage_cmd), id, "%s.operate-power-percentagecmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->reset_alarms_cmd), id, "%s.reset-alarms-cmd", name));
//    PIN(hal_pin_u32_newf(HAL_IN, &(h->apd_modes_index_cmd), id, "%s.apd-modes-indexcmd", name));
//    PIN(hal_pin_bit_newf(HAL_IN, &(h->save_apd_modes_index_cmd), id, "%s.save-apd_modes-indexcmd", name));
    PIN(hal_pin_float_newf(HAL_IN, &(h->apd_mode_index_cmd), id, "%s.apd-index-cmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->enabled), id, "%s.enabled", name));

    return 0;
}
#undef PIN

static int set_defaults(param_pointer p)
{
    haldata_t *h = p->haldata;

    *(h->status) = 0;
    *(h->device_temperature) = 0.0;
//    *(h->digital_interface_status) = 0;
    *(h->extend_status) = 0;
//    *(h->br_counter) = 0;
    *(h->modbus_ok) = false;

    *(h->guide_cmd) = false;
    *(h->ee_cmd) = false;
    *(h->emission_cmd) = false;
    *(h->apd_mode_index_cmd) = 0;
    *(h->prr_cmd) = 0.0;
    *(h->operate_power_percentage_cmd) = 0.0;

    h->looptime = 0.05;
    h->speed_tolerance = 0.01;      // output frequency within 1% of target frequency

    return 0;
}

int main(int argc, char **argv)
{
    struct timespec loop_timespec, remaining;
    int opt;
    param_pointer p = &param;
    int retval = 0;
    retval = -1;
    p->progname = argv[0];
    connection_state = NOT_CONNECTED;
    p->inifile = getenv("INI_FILE_NAME");

    while ((opt = getopt_long(argc, argv, option_string, long_options, NULL)) != -1)
    {
        switch(opt)
        {
            case 'n':
                p->modname = strdup(optarg);
                break;
            case 'm':
//                p->modbus_debug = 1;
                break;
            case 'd':
//                p->debug = 1;
                break;
            case 'S':
                p->section = optarg;
                break;
            case 'I':
                p->inifile = optarg;
                break;
            case 'r':
                p->report_device = 1;
                break;
            case 'h':
            default:
//      usage(argc, argv);
                exit(0);
        }
    }

    if (p->inifile)
    {
        if (read_ini(p))
            goto finish;
        if (!p->modname)
            p->modname = "ipglaser";
    }
    else
    {
        fprintf(stderr, "%s: ERROR: no inifile - either use '--ini inifile' or set INI_FILE_NAME environment variable\n", p->progname);
        goto finish;
    }

    signal(SIGINT, quit);
    signal(SIGTERM, quit);
    signal(SIGUSR1, toggle_debug);
    signal(SIGUSR2, toggle_modbus_debug);

    // create HAL component
    p->hal_comp_id = hal_init(p->modname);
    if ((p->hal_comp_id < 0) || (connection_state == DONE))
    {
        fprintf(stderr, "%s: ERROR: hal_init(%s) failed: HAL error code=%d\n",
                p->progname, p->modname, p->hal_comp_id);
        retval = p->hal_comp_id;
        goto finish;
    }

    // grab some shmem to store the HAL data in
    p->haldata = (haldata_t *)hal_malloc(sizeof(haldata_t));
    if ((p->haldata == 0) || (connection_state == DONE))
    {
        fprintf(stderr, "%s: ERROR: unable to allocate shared memory\n", p->modname);
        retval = -1;
        goto finish;
    }
    if (hal_setup(p->hal_comp_id,p->haldata, p->modname))
        goto finish;

    set_defaults(p);
    hal_ready(p->hal_comp_id);

    DBG("using libmodbus version %s\n", LIBMODBUS_VERSION_STRING);

    switch (p->type)
    {

        case TYPE_CUSTOM:
            connection_state = OPENING;
            p->fd = open(p->device,O_RDWR|O_NOCTTY|O_EXCL|O_NDELAY); // |O_NDELAY 
            if(p->fd < 0)
            {
                fprintf(stderr, "%s:%s ERROR",p->device,p->progname);
            }
            tcgetattr(p->fd, &old_tios);
            int ret = set_port_attr(p->fd,p->baud,p->bits,p->stopbits,p->parity,1,255);
            if(ret < 0)
            {
                fprintf(stderr, "%s:%s ERROR param Setting",p->device,p->progname);
            }
            sleep(p->reconnect_delay);
            tcflush(p->fd, TCIOFLUSH);//clear input and output buffer

            break;

        default:
            fprintf(stderr, "%s: ERROR: invalid connection type %d\n",
                    p->progname, p->type);
            goto finish;
    }

    connection_state = CONNECTED;
    while (connection_state != DONE)
    {

        while (connection_state == CONNECTED)
        {
            if ((retval = read_data(p->fd, p->haldata, p)))
            {
                p->modbus_ok = 0;
            }
            else
            {
                p->modbus_ok++;
            }
            if (p->modbus_ok > MODBUS_MIN_OK)
            {
                *(p->haldata->modbus_ok) = 1;
            }
            else
            {
                *(p->haldata->modbus_ok) = 0;
            }
            if ((retval = write_data(p->fd, p->haldata, p)))
            {
                p->modbus_ok = 0;
                if (retval == 1)
                {
                    connection_state = RECOVER;
                }
            }
            else
            {
                p->modbus_ok++;
            }
            if (p->modbus_ok > MODBUS_MIN_OK)
            {
                *(p->haldata->modbus_ok) = 1;
            }
            else
            {
                *(p->haldata->modbus_ok) = 0;
            }
            /* don't want to scan too fast, and shouldn't delay more than a few seconds */
            if (p->haldata->looptime < 0.001) p->haldata->looptime = 0.001;
            if (p->haldata->looptime > 1) p->haldata->looptime = 1;
            loop_timespec.tv_sec = (time_t)(p->haldata->looptime);
            loop_timespec.tv_nsec = (long)((p->haldata->looptime - loop_timespec.tv_sec) * 1000000000l);
            nanosleep(&loop_timespec, &remaining);
        }

        switch (connection_state)
        {
            case DONE:
                // cleanup actions before exiting.
                tcflush(p->fd, TCIOFLUSH);//clear input and output buffer
                DBG("cleanup actions before exiting");
                break;

            case RECOVER:
                DBG("recover\n");
                set_defaults(p);
                p->read_initial_done = 0;
                // reestablish connection to slave
                switch (p->type)
                {

                    case TYPE_RTU:
                    case TYPE_TCP_CLIENT:
                        DBG("rtu/tcp reconnect\n");
                        break;

                    case TYPE_CUSTOM:
                        ioctl(p->fd, TCFLSH, 2);  //clear input and output buffer
                        close(p->fd);
                        p->fd = -1;
                        while ((connection_state != CONNECTED) &&
                               (connection_state != DONE))
                        {
                            sleep(p->reconnect_delay);
                            p->fd = open(p->device,O_RDWR|O_NDELAY|O_NOCTTY|O_EXCL);
                            if(p->fd < 0)
                            {
                                fprintf(stderr, "%s:%s ERROR",p->device,p->progname);
                            }
                            else
                            {
                                connection_state = CONNECTED;
                                tcgetattr(p->fd, &old_tios);
                                int ret = set_port_attr(p->fd,p->baud,p->bits,p->stopbits,p->parity,1,255);
                                if(ret < 0)
                                {
                                    fprintf(stderr, "%s:%s ERROR param Setting",p->device,p->progname);
                                }
                                sleep(p->reconnect_delay);
                                tcflush(p->fd, TCIOFLUSH);//clear input and output buffer
                                DBG("custom reconnect\n");
                            }
                            
                        }
                        break;

                    case TYPE_TCP_SERVER:
                        DBG("tcpserver reconnect\n");
                        break;

                    default:
                        break;
                }
                break;
            default:
                ;
                break;
        }
    }
    retval = 0;

finish:
    windup(p);
    return retval;
}


